#include <stdio.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>

#include "hx3690l.h"
#include "hx3690l_hrv_agc.h"
#include "tyhx_hrv_alg.h"

#ifdef TYHX_DEMO
//#include "twi_master.h"
//#include "SEGGER_RTT.h"
#endif

#ifdef HRV_ALG_LIB

extern const uint8_t  hx3690l_hrs_agc_idac;
extern const uint8_t  green_led_max_init;
extern uint8_t low_vbat_flag;

//hrv_INFRARED_THRES
extern const int32_t  hrv_ir_unwear_thres; 
extern const int32_t  hrv_ir_wear_thres; 

static uint8_t s_ppg_state = 0;
static uint8_t s_cal_state = 0; 
static int32_t agc_buf[4] = {0};

static uint8_t cal_delay = HRV_CAL_DELAY_COUNT;
static HRV_CAL_SET_T  calReg;
//
static hrv_wear_msg_code_t hrv_wear_status = MSG_HRV_NO_WEAR;
static hrv_wear_msg_code_t hrv_wear_status_pre = MSG_HRV_NO_WEAR;

static uint8_t no_touch_cnt = 0;

hrv_wear_msg_code_t hx3690_hrv_wear_mode_check(int32_t infrared_data)
{ 
    if(infrared_data > hrv_ir_wear_thres)
    {
        if(no_touch_cnt < NO_TOUCH_CHECK_NUM)
        {
            no_touch_cnt++;
        }
        if(no_touch_cnt >= NO_TOUCH_CHECK_NUM)  
        {
            hrv_wear_status = MSG_HRV_WEAR;
        }   
    }
    else if(infrared_data < hrv_ir_unwear_thres)
    {
        if(no_touch_cnt>0)
        {
            no_touch_cnt--;
        }
        if(no_touch_cnt == 0)
        {
            hrv_wear_status = MSG_HRV_NO_WEAR;                
        }      
    }

    if(hrv_wear_status_pre != hrv_wear_status)
    {
        hrv_wear_status_pre = hrv_wear_status;
        if(hrv_wear_status_pre == MSG_HRV_NO_WEAR)
        {
            hx3690l_hrv_low_power();                 
        }
        else if(hrv_wear_status_pre == MSG_HRV_WEAR)
        {
            tyhx_hrv_alg_open_deep();
            hx3690l_hrv_set_mode(PPG_INIT);
            hx3690l_hrv_set_mode(CAL_INIT);
        }  
    }
    
    return hrv_wear_status;
}

void Init_hrv_PPG_Calibration_Routine(HRV_CAL_SET_T *calR,uint8_t led)
{
    calR->flag = CAL_FLG_LED_DR|CAL_FLG_LED_DAC|CAL_FLG_AMB_DAC|CAL_FLG_RF;
    
    calR->LEDDAC = 0;   /* 0~127 = 0 ~ 32ua , step = 0.25ua */
    calR->AMBDAC = 0;   /* 0~127 = 0 ~ 32ua , step = 0.25ua */
    calR->RF = 0;       /* 0= 10K; 1= 20k; 2= 50k; 3= 100k; 4= 150k; 5= 200k; 6= 500k; 7= 1M*/
    calR->LED = HRV_CAL_INIT_LED;
    calR->state = hrvCalStart;
    calR->int_cnt = 0;
    calR->cur255_cnt =0;
    if(low_vbat_flag==1)
    {
       calR->led_max_cur = 128;
    }
    else
    {
       calR->led_max_cur = green_led_max_init;
    }
}

void Restart_hrv_PPG_Calibration_Routine(HRV_CAL_SET_T *calR)
{
    calR->flag = CAL_FLG_LED_DR|CAL_FLG_LED_DAC|CAL_FLG_AMB_DAC|CAL_FLG_RF;
    calR->state = hrvCalLed;
    calR->int_cnt = 0;
}

void PPG_hrv_Calibration_Routine(HRV_CAL_SET_T *calR, int32_t led, int32_t amb)
{
    switch(calR->state)
    {
		case hrvCalStart:
			calR->state = hrvLedCur;
			break;
				
        case hrvLedCur:
			if(amb>1060000)
			{
				calR->AMBDAC = (amb-1060000)/2600;
			}
			else
			{
				calR->AMBDAC = 0;
			}
            if(led>amb+256)
            {             
                calR->led_step = (led-amb)/HRV_CAL_INIT_LED;
                calR->LED = 2600*(hx3690l_hrs_agc_idac+calR->AMBDAC)/calR->led_step;
                if(calR->LED>calR->led_max_cur)
                {
                    calR->LED = calR->led_max_cur;
                }
                else if(calR->LED < 64)
                {
                    calR->LED = 64;
                }
                if(calR->LED != calR->led_max_cur)
                {
                    calR->LEDDAC = (hx3690l_hrs_agc_idac*3)>>2;
                }
                else
                {
                    calR->LEDDAC = hx3690l_hrs_agc_idac>>1;
                }
                calR->RF = 6;
                calR->flag = CAL_FLG_LED_DR|CAL_FLG_LED_DAC|CAL_FLG_AMB_DAC|CAL_FLG_RF;           
                calR->state = hrvCalLed;
            }          
            else
            {
                calR->state = hrvCalFinish;
            }
            break;
						
        case hrvCalLed:   
            if(led>1300000 && calR->LEDDAC<=120)
            {
                calR->LEDDAC = calR->LEDDAC + 2;
                calR->state = hrvCalLed;
            }
            else if(led<800000 && calR->LEDDAC>=2)
            {
                calR->LEDDAC = calR->LEDDAC - 2;
                calR->state = hrvCalLed;
            }
			else if(led<800000 && calR->LEDDAC>0)
			{
				calR->LEDDAC = calR->LEDDAC - 1;
				calR->state = hrvCalLed;
			}
            else
            {
                calR->state = hrvCalLed2;
            }    
            calR->flag = CAL_FLG_LED_DAC;            
            break;
						
       case hrvCalLed2:   
            if(led+calR->LEDDAC*130000 < (amb+calR->AMBDAC*130000)+((130000*hx3690l_hrs_agc_idac*3)>>2) && calR->LED < calR->led_max_cur)
            {
                calR->LED = calR->led_max_cur;
                calR->state = hrvCalLed;
                calR->flag = CAL_FLG_LED_DR;
            }
            else
            {
                calR->state = hrvCalFinish;
                calR->flag = 0;
            }              
            break;
				
        default:
            break;
        
    }
    AGC_LOG("AGC: led_drv=%d,ledDac=%d,ambDac=%d,ledstep=%d,rf=%d,\r\n",\
            calR->LED, calR->LEDDAC, calR->AMBDAC,calR->led_step,calR->RF);
}

HRV_CAL_SET_T PPG_hrv_agc(void)
{
    int32_t led_val, amb_val;
    calReg.work = false;
    if (!s_cal_state) 
    {
        return  calReg;
    } 

	#ifdef INT_MODE     
	calReg.int_cnt ++;
	if(calReg.int_cnt < 8)
	{  
		 return calReg;
	}
	calReg.int_cnt = 0;
	hx3690l_gpioint_cfg(false);
	#endif  
		
	calReg.work = true;   
	hx3690l_hrv_read_data_packet(agc_buf);
	led_val = agc_buf[0];
	amb_val = agc_buf[1];
    
    AGC_LOG("cal dat ch1=%d,ch2=%d,led_val=%d,amb_val=%d \r\n",\
    agc_buf[0], agc_buf[1], led_val, amb_val);
    
    PPG_hrv_Calibration_Routine(&calReg, led_val, amb_val);
    
    if (calReg.state == hrvCalFinish)
	{
		hx3690l_hrv_set_mode(CAL_OFF);
		#if defined(TIMMER_MODE)
		#else
		hx3690l_gpioint_cfg(true);
		#endif
    }
	else
	{
		hx3690l_hrv_updata_reg();
		#if defined(INT_MODE)
		hx3690l_gpioint_cfg(true);
		#endif
	}
    return  calReg;
}

void hx3690l_hrv_cal_init(void) // 20200615 ericy afe cali online
{
	uint16_t sample_rate = 200;                      /*config the data rate of chip alps2_fm ,uint is Hz*/
	uint32_t prf_clk_num = 32000/sample_rate;        /*period in clk num, num = Fclk/fs */

	hx3690l_write_reg(0X1a, (uint8_t)prf_clk_num);    // prf bit<7:0>
	hx3690l_write_reg(0X1b, (uint8_t)(prf_clk_num>>8)); // prf bit<15:8>
	hx3690l_write_reg(0X1c, (uint8_t)(prf_clk_num>>16)); // prf bit<23:16>

	hx3690l_write_reg(0x13,0x10); 
	hx3690l_write_reg(0x23,0x20); //phase4 convertion ready enable

	#if defined(INT_MODE)
	hx3690l_write_reg(0x24,0x00);
	#endif

	hx3690l_write_reg(0x51,0x02); //Chip reset
	hx3690l_delay_ms(5);          //Delay for reset time
	hx3690l_write_reg(0x51,0x00); //Chip state machine work normal
	hx3690l_delay_ms(5);
}

void hx3690l_hrv_cal_off(void) // 20200615 ericy afe cali offline
{
	uint16_t sample_rate = 125;                      /*config the data rate of chip alps2_fm ,uint is Hz*/
	uint32_t prf_clk_num = 32000/sample_rate;        /*period in clk num, num = Fclk/fs */
	hx3690l_write_reg(0X1a, (uint8_t)prf_clk_num);    // prf bit<7:0>
	hx3690l_write_reg(0X1b, (uint8_t)(prf_clk_num>>8)); // prf bit<15:8>
	hx3690l_write_reg(0X1c, (uint8_t)(prf_clk_num>>16)); // prf bit<23:16>

	hx3690l_write_reg(0x13,0x11); //FIFO mode enable
	hx3690l_write_reg(0x23,0x00); //phase3 convertion ready disable	

	#if defined(INT_MODE)
	hx3690l_write_reg(0x24,0x20); 		
	#endif

	hx3690l_write_reg(0x51,0x02); //Chip reset
	hx3690l_delay_ms(5);          //Delay for reset time
	hx3690l_write_reg(0x51,0x00); //Chip state machine work normal
	hx3690l_delay_ms(5); 
}

void read_hrv_data_packet(int32_t *buf) 
{
    uint8_t dataBuf[6];
    
    hx3690l_brust_read_reg(0x03, dataBuf, 3); 
    hx3690l_brust_read_reg(0x0c, dataBuf+3, 3);    
    
    for (uint8_t i=0; i<2; i++) 
    {
        buf[i] = (int32_t)(dataBuf[3*i]|(dataBuf[3*i+1]<<8)|(dataBuf[3*i+2]<<16));
    }
}

void read_hrv_ir_packet(int32_t *buf)
{
    uint8_t dataBuf[6];
    
    hx3690l_brust_read_reg(0x06, dataBuf, 6);
    
    for (uint8_t i=0; i<2; i++) 
    {
        buf[i] = (int32_t)(dataBuf[3*i]|(dataBuf[3*i+1]<<8)|(dataBuf[3*i+2]<<16));
    }
}

void hx3690l_hrv_read_data_packet(int32_t *buf)
{
	uint8_t  databuf1[6] = {0};
	uint8_t  databuf2[6] = {0};
	uint32_t P1 = 0,P2 = 0 ,P3 = 0 ,P4 =0 ; 

	hx3690l_brust_read_reg(0x03, databuf1, 6); 
	hx3690l_brust_read_reg(0x09, databuf2, 6); 		

	P1 = ((databuf1[0])|(databuf1[1]<<8)|(databuf1[2]<<16)); 
	P3 = ((databuf1[3])|(databuf1[4]<<8)|(databuf1[5]<<16)); 
	P4 = ((databuf2[0])|(databuf2[1]<<8)|(databuf2[2]<<16));	
	P2 = ((databuf2[3])|(databuf2[4]<<8)|(databuf2[5]<<16));

	buf[0] = P1;
	buf[1] = P2;
	buf[2] = P3;
	buf[3] = P4;
}

void hx3690l_hrv_low_power(void)
{   
    uint16_t sample_rate = 10;                       /*config the data rate of chip alps2_fm ,uint is Hz*/
    uint8_t data_avg_num = 0;                       /* 0 = 1 ; 1 = 2; 2 =4 ; 3 =8 ; 4 =16 ;*/
    uint32_t prf_clk_num = 32000/sample_rate;       /*period in clk num, num = Fclk/fs */
    
    uint8_t phase1_tia_res = 0;     /* 0= 10K; 1= 20k; 2= 50k; 3= 100k; 4= 150k; 5= 200k; 6= 500k; 7= 1M*/
    uint8_t phase1_offset_idac = 0; /* 0~127 = 0 ~ 32ua , step = 0.25ua */
    uint8_t phase1_ldr_cur = 0; 
    uint8_t phase1_led_en = 0;    
    uint8_t phase2_led_en = 0;
    uint8_t phase3_led_en = 1;
    uint8_t phase4_led_en = 0;
    
    uint8_t phase3_tia_res = 0; 
    uint8_t phase4_tia_res = 0; 
    
    uint8_t phase3_ldr_cur = 64;  /* 0~255 = 0 ~ 200ma, step = 0.8ma */
    
    uint8_t led_on_time = 1;      /* 0 = 32clk=8us ; 1 = 64clk=16us; 2=128clk=32us ; 3 = 256clk=64us ;
                                     4 = 512clk=128us ; 5 = 1024clk=256us; 6= 2048clk=512us; 7 = 4096clk=1024us */
    
    hx3690l_write_reg(0X1a, (uint8_t)prf_clk_num);    // prf bit<7:0>
    hx3690l_write_reg(0X1b, (uint8_t)(prf_clk_num>>8)); // prf bit<15:8>
    hx3690l_write_reg(0X1c, (uint8_t)(prf_clk_num>>16)); // prf bit<23:16>
    hx3690l_write_reg(0X3c, data_avg_num<<4 | data_avg_num );
    hx3690l_write_reg(0X3d, data_avg_num<<4 | data_avg_num );	
    
    hx3690l_write_reg(0X2d, phase3_tia_res);
    hx3690l_write_reg(0X2e, phase4_tia_res);

	hx3690l_write_reg(0X2c, phase1_tia_res); 
	hx3690l_write_reg(0X38, phase1_offset_idac);
	hx3690l_write_reg(0X30, phase1_ldr_cur);  
	hx3690l_write_reg(0X31, phase3_ldr_cur);
	hx3690l_write_reg(0X1f, (led_on_time<<4| phase1_led_en<<3 | phase3_led_en<<2 | phase4_led_en<<1 | phase2_led_en) );
	hx3690l_write_reg(0X69, 0xa2);	//bit<0>: rc_comb_en bits<1>=rc_rbp_en bits<7>= vcom_clamp_en bits<6:4>= LED_vdesl

	#ifdef INT_MODE
	hx3690l_write_reg(0X12,0x05); //fifo almostfull cfg ,max=0x40;
	hx3690l_write_reg(0x13,0x11); //FIFO bypass mode enable
	hx3690l_write_reg(0x23,0x00); //phase convertion ready disable
	#else
	hx3690l_write_reg(0x13,0x11); //FIFO bypass mode enable
	hx3690l_write_reg(0x23,0x00); //phase convertion ready disable
	#endif

	hx3690l_write_reg(0x51,0x02); //Chip reset
	hx3690l_delay_ms(5);          //Delay for reset time
	hx3690l_write_reg(0x51,0x00); //Chip state machine work normal
	hx3690l_delay_ms(5); 

	calReg.LED =  phase1_ldr_cur;
	AGC_LOG(" chip go to low power mode  \r\n" );    
}

void hx3690l_hrv_updata_reg(void)
{
    if (calReg.flag & CAL_FLG_LED_DR) 
    {
        hx3690l_write_reg(0X30, calReg.LED);     // phase1 led driver config
    }
    
    if (calReg.flag & CAL_FLG_LED_DAC) 
    {
        hx3690l_write_reg(0X38, calReg.LEDDAC);  // phase1 offset idac cfg
    }
    
    if (calReg.flag & CAL_FLG_AMB_DAC) 
    {
        hx3690l_write_reg(0X3b, calReg.AMBDAC);  // phase2 offset idac cfg
    }
    
    if (calReg.flag & CAL_FLG_RF) 
    {
        hx3690l_write_reg(0X2c, calReg.RF);    // phase1 tia feed back resister cfg
        hx3690l_write_reg(0X2f, calReg.RF);    // phase2 tia feed back resister cfg
    }
}

void hx3690l_hrv_set_mode(uint8_t mode_cmd)
{
    switch (mode_cmd) 
    {
        case PPG_INIT:
            hx3690l_hrv_ppg_init();
			#if defined(TIMMER_MODE)
			hx3690l_320ms_timer_cfg(true);
                #if defined(GSEN_40MS_TIMMER)
                hx3690l_40ms_timer_cfg(true);
                #endif
			#else
				hx3690l_gpioint_cfg(true);
			#endif
            s_ppg_state = 1;

            AGC_LOG("ppg init mode\r\n");
            break;

        case PPG_OFF:
            hx3690l_ppg_off();
            s_ppg_state = 0;
            AGC_LOG("ppg off mode\r\n");
            break;
				
        case PPG_LED_OFF:
            hx3690l_hrv_low_power();
            s_ppg_state = 0;
            AGC_LOG("ppg led off mode\r\n");
            break;

        case CAL_INIT:
            Init_hrv_PPG_Calibration_Routine(&calReg, 64);
            hx3690l_hrv_cal_init();
            hx3690l_hrv_updata_reg();
            #if defined(TIMMER_MODE)
                #if defined(GSEN_40MS_TIMMER)
                #else
                hx3690l_40ms_timer_cfg(true);
                #endif
            #endif 
            s_cal_state = 1;
            AGC_LOG("cal init mode\r\n");   
            break;   
				
        case RECAL_INIT:        
            Restart_hrv_PPG_Calibration_Routine(&calReg);
            hx3690l_hrv_cal_init();
            hx3690l_hrv_updata_reg();
            #if defined(TIMMER_MODE)
                #if defined(GSEN_40MS_TIMMER)
                #else
                hx3690l_40ms_timer_cfg(true);
                #endif
            #endif 
            s_cal_state = 1;
            AGC_LOG("Recal init mode\r\n");
            break;

        case CAL_OFF:
            #if defined(TIMMER_MODE)
                #if defined(GSEN_40MS_TIMMER)
                #else
                hx3690l_40ms_timer_cfg(false);
                #endif
            #endif
            hx3690l_hrv_cal_off();
            s_cal_state = 0;
            AGC_LOG("cal off mode\r\n");
            break;

        default:
            break;
    }
}

SENSOR_ERROR_T hx3690l_hrv_enable(void)
{
    if (!hx3690l_chip_check()) 
    {
        AGC_LOG("hx3690l check id failed!\r\n");
        return SENSOR_OP_FAILED;
    }

    AGC_LOG("hx3690l check id success!\r\n");

    if (s_ppg_state) 
    {
        AGC_LOG("ppg already on!\r\n");
        return SENSOR_OP_FAILED;
    }
    if(!tyhx_hrv_alg_open())
    {
        AGC_LOG("hrv alg open fail,or dynamic ram not enough!\r\n");
    }
    
    hrv_wear_status = MSG_HRV_NO_WEAR;
    hrv_wear_status_pre = MSG_HRV_NO_WEAR;
    
    hx3690l_hrv_set_mode(PPG_INIT);

    AGC_LOG("hx3690l enable!\r\n");

    return SENSOR_OK;
}


void hx3690l_hrv_disable(void)
{
    hx3690l_320ms_timer_cfg(false);
    hx3690l_40ms_timer_cfg(false);
	
    hx3690l_hrv_set_mode(CAL_OFF);
    hx3690l_hrv_set_mode(PPG_OFF);
    tyhx_hrv_alg_close();

    AGC_LOG("hx3690l disable!\r\n");
}

void hx3690l_hrv_data_reset(void)
{
    s_ppg_state = 0;
    s_cal_state = 0;
    tyhx_hrv_alg_close();
}

hrv_wear_msg_code_t hx3690_hrv_get_wear_status(void)
{
    return  hrv_wear_status;
}

hrv_wear_msg_code_t hx3690_hrv_get_wear_status_pre(void)
{
    return  hrv_wear_status_pre;
}

HRV_CAL_SET_T get_hrv_agc_status(void)
{
    HRV_CAL_SET_T cal;

    cal.flag =  calReg.flag;
    cal.int_cnt =  calReg.int_cnt;
    cal.LED=  calReg.LED;     // phasex led driver config
    cal.LEDDAC=  calReg.LEDDAC;  // phasex led offset idac cfg
    cal.AMBDAC=  calReg.AMBDAC;  // phasex offset idac cfg
    cal.RF=  calReg.RF;      // phasex tia feed back resister cfg
    cal.led_step=  calReg.led_step;
    cal.state=  calReg.state;

    return cal;
}

void hx3690l_hrv_read_fifo_data(uint8_t read_fifo_size,int32_t *buf)
{
    uint8_t data_flg = 127;
    int32_t data;
    uint8_t databuf[3];
    uint8_t ii=0;
    for(ii=0; ii<read_fifo_size; ii++) 
    {
        hx3690l_write_reg(0x17, 0x00); // write any walue to 0x17 will update a new data
        hx3690l_delay_us(100);
		hx3690l_brust_read_reg(0x15, databuf, 3); 
        data_flg = databuf[2]>>5;
        data = (int32_t)(databuf[0]|(databuf[1]<<8)|((databuf[2]&0x1f)<<16));

        if(ii==0)
		{
			if(data_flg ==3)
			{
				ii=3;
				buf[0] = 0;
				buf[1] = 0;
				buf[2] = 0;
			}
			if(data_flg ==2)
			{      
				ii=2;
				buf[0] = 0;
				buf[1] = 0;
			}
			if(data_flg ==1)
			{    
				ii=1;
				buf[0] = 0;
			}			
		}

		if(data_flg == 0) 
		{
			buf[ii]= data;
		} 
		else if(data_flg == 1)
		{
			buf[ii]= data;
		} 
		else if(data_flg == 2)
		{
			buf[ii]= data;
		} 
		else if(data_flg == 3) 
		{
			buf[ii]= data;
		}  
	}
}

uint8_t hx3690l_hrv_read(ppg_sensor_data_t *s_dat)
{
    uint8_t i = 0;
	int32_t PPG_src_data = 0;
    int32_t Ir_src_data = 0;
	bool recal = false;
	uint8_t size = 0;
    uint8_t fifo_out_size = 0;
	int32_t *PPG_buf =  &(s_dat->green_data[0]);
	int32_t *ir_buf =  &(s_dat->ir_data[0]);
	int32_t *s_buf =  &(s_dat->s_buf[0]);
	s_dat->green_cur =  calReg.LED;
   int32_t ps_data[4];
    
    if (!s_ppg_state || s_cal_state) 
    {
        return 0;
    }    
    size = hx3690l_read_fifo_size();
   
    if(size<1)
    {
        return 0;
    }
    
    hx3690l_hrv_read_data_packet(ps_data);
	Ir_src_data = ps_data[2]-ps_data[3];
//    DEBUG_PRINTF("%d %d \r\n" ,ps_data[2],ps_data[3]);
    ir_buf[0] = Ir_src_data;  			
//    DEBUG_PRINTF("111: %d \r\n" ,ir_buf[0] );	
 	hx3690_hrv_wear_mode_check(Ir_src_data);
		
    if (size && size <= 64) 
    {
        fifo_out_size = hx3690l_read_fifo_data(s_buf,size,1);
        s_dat->count = fifo_out_size;
        for (i=0; i<fifo_out_size; i++) 
        {
            PPG_src_data = s_buf[i];            
            if (s_buf[i]<500000 || s_buf[i]>1500000)            
            {
                recal = true;
               
                if(hrv_wear_status==MSG_HRV_NO_WEAR)
                {
                     recal = false;
                }                
            }
            PPG_buf[i] = PPG_src_data; 
			//AGC_LOG("%d/%d %d %d %d\r\n" ,1+i,size, s_buf[i],calReg.LED,recal);						
        }
				
        if (recal) 
        {
            cal_delay--;
            if (cal_delay <= 0) 
            {
                cal_delay = HRV_CAL_DELAY_COUNT;
                hx3690l_hrv_set_mode(RECAL_INIT);
            }
        }
        else                       
        {
            cal_delay = HRV_CAL_DELAY_COUNT;
        }
    }   
    return 1;
}

#endif
